#!/bin/bash

GMSL_MODE=  # 0:3G mode; 1:6G mode
TEST_MODE=0 # 0:OFF ; 1:ON

function get_gmsl_mode(){
    local VALUE

    if [ ${TEST_MODE} -eq 0 ];then
	VALUE=$(i2ctransfer -f -y 6 w2@0x48 0x00 0x01 r1)
    else
	VALUE=0x82 # for test
    fi

    if [ $((${VALUE} & 0x3)) -eq 1 ]; then
	GMSL_MODE=0
    else
	if [ $((${VALUE} & 0x3)) -eq 2 ];then
	     GMSL_MODE=1
	else
	    echo -e "\033[41;37mDES UNKNOWN LINK STATE.\033[0m"
	    exit
	fi
    fi
}

function check_link_3g(){
    local VALUE

    if [ ${TEST_MODE} -eq 0 ];then
	VALUE=$(i2ctransfer -f -y 6 w2@0x48 0x0b 0xcb r1)
    else
	VALUE=0x1
    fi

    if [ $(( ${VALUE} & 1 )) -ne 0 ];then
	echo -e "Camera GMSL1 Link Locked."
    else
	VALUE=$(i2ctransfer -f -y 6 w2@0x48 0x00 0x13 r1)
	if [ $(( ${VALUE} & 0x8 )) -ne 0 ];then
	    echo -e "Camera GMSL2 Link Locked."
	else
	    echo -e "\033[41;37mCAMERA Link NOT LOCKED.\033[0m"
	    exit
	fi
    fi
}

function check_link_6g(){
    local VALUE

    if [ ${TEST_MODE} -eq 0 ];then
	VALUE=$(i2ctransfer -f -y 6 w2@0x48 0x00 0x13 r1)
    else
	VALUE=0x8
    fi


    if [ $(( VALUE & 8 )) -ne 0 ];then
	echo -e "Camera GMSL2 Link Locked."
    else
	echo -e "\033[41;37mCAMERA Link NOT LOCKED.\033[0m"
	exit
    fi
}

function check_video_channel(){
    local PIPE_X
    local PIPE_Y
    local PIPE_Z
    local PIPE_U

    if [ ${TEST_MODE} -eq 0 ];then
	PIPE_X=$(i2ctransfer -f -y 6 w2@0x48 0x01 0xdc r1)
	PIPE_Y=$(i2ctransfer -f -y 6 w2@0x48 0x01 0xfc r1)
	PIPE_Z=$(i2ctransfer -f -y 6 w2@0x48 0x02 0x1c r1)
	PIPE_U=$(i2ctransfer -f -y 6 w2@0x48 0x02 0x3c r1)
    else
	PIPE_X=0
	PIPE_Y=0
	PIPE_Z=0
	PIPE_U=0
    fi

    PIPE_X=$(( $PIPE_X & 1 ))
    PIPE_Y=$(( $PIPE_Y & 1 ))
    PIPE_Z=$(( $PIPE_Z & 1 ))
    PIPE_U=$(( $PIPE_U & 1 ))
    PIPE_ALL=$(( $PIPE_X | $PIPE_Y | $PIPE_Z | $PIPE_U ))
    
    if [ $(( ${PIPE_ALL} & 1 )) -eq 0 ];then
	echo -e "\033[41;37mNO PIPE LOCKED.Camera is not initialized.\033[0m"
    else
	if [ $PIPE_X -eq 1 ];then
	    echo -e "PIPE X Locked."
	fi
	if [ $PIPE_Y -eq 1 ];then
	    echo -e "PIPE Y Locked."
	fi
	if [ $PIPE_Z -eq 1 ];then
	    echo -e "PIPE Z Locked."
	fi
	if [ $PIPE_U -eq 1 ];then
	    echo -e "PIPE U Locked."
	fi
    fi
}

# main
get_gmsl_mode
if [ ${GMSL_MODE} -eq 0 ];then
    check_link_3g
else
    check_link_6g
fi
check_video_channel